{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "from local_plot import *\n",
    "from utils import *\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj_vo1, t0 = read_path_from_csv(f\"/home/xuhao/data/d2slam/handhold/handhold1-2/d2vins_1.csv\")\n",
    "traj_vo2, _ = read_path_from_csv(f\"/home/xuhao/data/d2slam/handhold/handhold2-2/d2vins_2.csv\", t0=t0)\n",
    "# traj_pgo1, t0 = read_path_from_csv(f\"/home/xuhao/data/d2slam/handhold/handhold1-2/pgo_1.csv\")\n",
    "# traj_pgo2, _ = read_path_from_csv(f\"/home/xuhao/data/d2slam/handhold/handhold2-2/pgo_2.csv\", t0=t0)\n",
    "\n",
    "trajs_vo = {\n",
    "    1: traj_vo1,\n",
    "    2: traj_vo2,\n",
    "}\n",
    "\n",
    "# trajs_pgo = {\n",
    "#     1: traj_pgo1,\n",
    "#     2: traj_pgo2,\n",
    "# }\n",
    "\n",
    "nodes = [1, 2]\n",
    "plot_fused(nodes, trajs_vo)\n",
    "# plot_fused_err(nodes, trajs_vo, trajs_pgo)\n",
    "plot_relative_pose_err(1, [2], trajs_vo, trajs_vo, common_time_dt=0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Extract compuation time from log.\n",
    "# First is the trackRemoteFrames\n",
    "# Sample log: \n",
    "# [D2FeatureTracker::trackRemoteFrames] frame 1000002, matched 168, time 2.88ms\n",
    "import re\n",
    "\n",
    "def extract_trackRemoteFrames_from_log(log_content):\n",
    "    # Sample log: \n",
    "    # [D2FeatureTracker::trackRemoteFrames] frame 1000002, matched 168, time 2.88ms\n",
    "    # Match using regex\n",
    "    pattern = re.compile(r\"\\[D2FeatureTracker::trackRemoteFrames\\] frame \\d+, matched \\d+, time (\\d+\\.\\d+)ms\")\n",
    "    matched = pattern.findall(log_content)\n",
    "    matched = np.array(matched, dtype=float)\n",
    "    return np.mean(matched)\n",
    "    \n",
    "def extract_trackLocal_from_log(log_content):\n",
    "    # Sample log: \n",
    "    # [D2FeatureTracker] frame_id: 2000072, landmark_num: 400, time_cost: 3.0ms\n",
    "    # Match using regex\n",
    "    pattern = re.compile(r\"\\[D2FeatureTracker\\] frame_id: \\d+, landmark_num: \\d+, time_cost: (\\d+\\.\\d+)ms\")\n",
    "    matched = pattern.findall(log_content)\n",
    "    matched = np.array(matched, dtype=float)\n",
    "    return np.mean(matched)\n",
    "\n",
    "def extract_LoopCam_from_log(log_content):\n",
    "    # Sample log: \n",
    "    # [D2LoopCam] [D2Frontend::LoopCam] KF Count 2574 loop_cam cost avg 41.7ms cur 49.5ms\n",
    "    # Match using regex, to match current loop_cam time\n",
    "    pattern = re.compile(r\"\\[D2Frontend::LoopCam\\] KF Count \\d+ loop_cam cost avg \\d+\\.\\d+ms cur (\\d+\\.\\d+)ms\")\n",
    "    matched = pattern.findall(log_content)\n",
    "    matched = np.array(matched, dtype=float)\n",
    "    return np.mean(matched)\n",
    "\n",
    "def extract_Margin_from_log(log_content):\n",
    "    # Sample log: \n",
    "    # [D2VINS::marginalize] time cost 83.6ms frame_id 1000079 total_eff_state_dim: 239 keep_size 118 remove size 121 eff_residual_size: 5289 keep_block_size 20 \n",
    "    # Match using regex, to match time cost\n",
    "    pattern = re.compile(r\"\\[D2VINS::marginalize\\] time cost (\\d+\\.\\d+)ms frame_id \\d+ total_eff_state_dim: \\d+ keep_size \\d+ remove size \\d+ eff_residual_size: \\d+ keep_block_size \\d+\")\n",
    "    matched = pattern.findall(log_content)\n",
    "    matched = np.array(matched, dtype=float)\n",
    "    return np.mean(matched)\n",
    "\n",
    "def extract_optimization_time_from_log(log_content):\n",
    "    # Sample log: \n",
    "    # [D2VINS::solveDist@2](532) odom Pose T [+0.004,+0.215,+0.075] YPR [-4.1,+3.9,+0.8] Vel -0.00 0.25 0.45@ref1 landmarks 100/100 drone_num 2 opti_time 62.6ms steps 3 td 1.0ms \n",
    "    # Match using regex, to match opti_time\n",
    "    pattern = re.compile(r\"\\[D2VINS::solveDist@2\\]\\(\\d+\\) odom Pose T \\[\\+0\\.\\d+,\\+0\\.\\d+,\\+0\\.\\d+\\] YPR \\[-\\d+\\.\\d+,\\+\\d+\\.\\d+,\\+\\d+\\.\\d+\\] Vel -?\\d+\\.\\d+ \\d+\\.\\d+ \\d+\\.\\d+@ref\\d+ landmarks \\d+/\\d+ drone_num \\d+ opti_time (\\d+\\.\\d+)ms steps \\d+ td \\d+\\.\\d+ms\")\n",
    "    matched = pattern.findall(log_content)\n",
    "    matched = np.array(matched, dtype=float)\n",
    "    return np.mean(matched)\n",
    "        \n",
    "def extract_comm_delay_from_log(log_content):\n",
    "    # Sample log:\n",
    "    # [LoopNet]ImageDescriptorHeader 1000108c0 from D1 delay 1.7ms msg_id -1970633955: feature num 155 gdesc 1024:0\n",
    "    # Match using regex, to match delay\n",
    "    pattern = re.compile(r\"\\[LoopNet\\]ImageDescriptorHeader \\d+c\\d from D\\d+ delay (\\d+\\.\\d+)ms msg_id -?\\d+: feature num \\d+ gdesc \\d+:\\d+\")\n",
    "    matched = pattern.findall(log_content)\n",
    "    matched = np.array(matched, dtype=float)\n",
    "    return np.mean(matched)\n",
    "\n",
    "def extract_track_rate_from_log(log_content):\n",
    "    # Sample log:\n",
    "    # [D2FeatureTracker::trackLK] track 82 LK points, 13 lost, track rate 84.1%\n",
    "    # Match using regex, to match track rate\n",
    "    pattern = re.compile(r\"\\[D2FeatureTracker::trackLK\\] track \\d+ LK points, \\d+ lost, track rate (\\d+\\.\\d+)%\")\n",
    "    matched = pattern.findall(log_content)\n",
    "    matched = np.array(matched, dtype=float)\n",
    "    return np.mean(matched)\n",
    "\n",
    "def extract_detect_time_from_log(log_content):\n",
    "    # Sample log:\n",
    "    # Lost 38 pts; Require 150 will detect 1\n",
    "    # Match using regex, to match detect time\n",
    "    pattern = re.compile(r\"\\[D2FeatureTracker::trackLK\\] detect (\\d+) points in (\\d+\\.\\d+)ms\")\n",
    "    # Only select if points > 0\n",
    "    matched = pattern.findall(log_content)\n",
    "    matched = [m for m in matched if int(m[0]) > 0]\n",
    "    matched = np.array(matched, dtype=float)\n",
    "    return np.mean(matched[:, 1])\n",
    "\n",
    "def extract_broadcast_D2VINS_from_log(log_content):\n",
    "    # Sample log:\n",
    "    # [D2VINS] Broadcast VINS Data size 4144 with 54 poses 10 extrinsic.\n",
    "    # Match using regex, to match broadcast poses and extrinsic\n",
    "    pattern = re.compile(r\"\\[D2VINS\\] Broadcast VINS Data size \\d+ with (\\d+) poses (\\d+) extrinsic.\")\n",
    "    matched = pattern.findall(log_content)\n",
    "    matched = np.array(matched, dtype=float)\n",
    "    return np.sum(matched[:, 0]) + np.sum(matched[:, 1])\n",
    "\n",
    "def extract_broadcast_D2PGO_from_log(log_content):\n",
    "    # [Drone 2] DPGO broadcast poses 249\n",
    "    # Match using regex, to match broadcast poses\n",
    "    pattern = re.compile(r\"\\[Drone \\d+\\] DPGO broadcast poses (\\d+)\")\n",
    "    matched = pattern.findall(log_content)\n",
    "    matched = np.array(matched, dtype=float)\n",
    "    return np.sum(matched)\n",
    "\n",
    "#Read log as string\n",
    "# log_vo = \"/home/xuhao/data/d2slam/handhold/handhold2-3/log_vo.txt\"\n",
    "# log_vo = \"/home/xuhao/data/d2slam/ri_realsense_walkaround_2022_10/outputs/d2vins-single/swarm1/d2vins.log\"\n",
    "log_vo = \"/home/xuhao/data/d2slam/tum_datasets/outputs/d2slam/swarm2/d2slam.log\"\n",
    "\n",
    "def process_logs(log_vo):\n",
    "    with open(log_vo, \"r\") as f:\n",
    "        log_content = f.read()\n",
    "        track_remote_time = extract_trackRemoteFrames_from_log(log_content)\n",
    "        print(f\"Averaged trackRemoteFrames time: {track_remote_time:.2f}ms\")\n",
    "        track_local_time = extract_trackLocal_from_log(log_content)\n",
    "        print(f\"Averaged trackLocal time: {track_local_time:.2f}ms\")\n",
    "        loop_cam_time = extract_LoopCam_from_log(log_content)\n",
    "        print(f\"Averaged LoopCam time: {loop_cam_time:.2f}ms\")\n",
    "        margin_time = extract_Margin_from_log(log_content)\n",
    "        print(f\"Averaged marginalization time: {margin_time:.2f}ms\")\n",
    "        opti_time = extract_optimization_time_from_log(log_content)\n",
    "        print(f\"Averaged optimization time: {opti_time:.2f}ms\")\n",
    "        comm_delay = extract_comm_delay_from_log(log_content)\n",
    "        print(f\"Averaged communication delay: {comm_delay:.2f}ms\")\n",
    "        track_rate = extract_track_rate_from_log(log_content)\n",
    "        print(f\"Averaged track rate: {track_rate:.2f}%\")\n",
    "        detect_time = extract_detect_time_from_log(log_content)\n",
    "        print(f\"Averaged detect time: {detect_time:.2f}ms\")\n",
    "        broadcast_D2VINS = extract_broadcast_D2VINS_from_log(log_content)\n",
    "        print(f\"Broadcast D2VINS: {broadcast_D2VINS/1024:.2f}\")\n",
    "        broadcast_D2PGO = extract_broadcast_D2PGO_from_log(log_content)\n",
    "        print(f\"Broadcast D2PGO: {broadcast_D2PGO*2/1024:.2f}\")\n",
    "        print(\"Sum of broadcast poses\", broadcast_D2VINS + broadcast_D2PGO*2)\n",
    "        return broadcast_D2VINS + broadcast_D2PGO*2\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.5 ('base')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "08ce52785f0fedc81003ce387e097a83d6cc9494681cd746006386992005bb71"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
